import serial
import threading
import time

data_queue = bytearray()
running = True
measure_interval = 0.2
last_measurement = 0
current_measurement = 0
last_time = time.time()
current_time = time.time()
data_mutex = threading.Lock()

class SerialWrapper:
    def __init__(self, port, baudrate):
        self.port = port
        self.baudrate = baudrate
        self.serial = serial.Serial()
        self.serial.port = port
        self.serial.baudrate = baudrate

    def open_serial(self):
        try:
            self.serial.open()
            print(f"Serial port configured: BaudRate = {self.baudrate}")
            return True
        except Exception as e:
            print(f"Error opening serial port: {e}")
            return False

    def close(self):
        if self.serial.is_open:
            self.serial.close()
            print("Serial port closed")

    def write_data(self, data):
        global running
        while running:
            try:
                self.serial.write(data)
                time.sleep(measure_interval)
            except Exception as e:
                if not running:
                    break
                print(f"Error writing to port: {e}")

    def read_data(self):
        global running
        while running:
            try:
                if self.serial.in_waiting > 0:
                    data = self.serial.read(self.serial.in_waiting)
                    self.process_received_data(data)
            except Exception as e:
                if not running:
                    break
                print(f"Error reading from port: {e}")

    def process_received_data(self, data):
        global data_queue, last_measurement, current_measurement, last_time, current_time
        with data_mutex:
            data_queue.extend(data)

            while len(data_queue) >= 7:
                if data_queue[0] == 0x01 and data_queue[1] == 0x03 and data_queue[2] == 0x02:
                    valid_data = data_queue[:7]
                    combined_value = (valid_data[3] << 8) | valid_data[4]
                    print(f"Measure Distance is: {combined_value} cm")

                    last_measurement = current_measurement
                    current_measurement = combined_value

                    last_time = current_time
                    current_time = time.time()

                    del data_queue[:7]
                else:
                    del data_queue[0]

def calculate_speed():
    global last_measurement, current_measurement, last_time, current_time, running
    while running:
        time.sleep(measure_interval)
        with data_mutex:
            if last_measurement != 0 and current_measurement != 0:
                duration = current_time - last_time
                if duration > 0:
                    distance_diff = current_measurement - last_measurement
                    time_diff = duration
                    speed = distance_diff / time_diff
                    if speed < 0:
                        print(f"Speed of approaching is: {abs(speed)} cm/s")
                    else:
                        print(f"Speed of moving away is: {abs(speed)} cm/s")

if __name__ == "__main__":
    print(" +----------------------------------+")
    print(" |        Serial Port Write         |")
    print(" +----------------------------------+")

    write_buffer = bytearray([0x01, 0x03, 0x00, 0x00, 0x00, 0x01, 0x84, 0x0A])

    serial_wrapper = SerialWrapper("/dev/ttyUSB0", 115200)

    if serial_wrapper.open_serial():
        send_thread = threading.Thread(target=serial_wrapper.write_data, args=(write_buffer,))
        receive_thread = threading.Thread(target=serial_wrapper.read_data)
        speed_thread = threading.Thread(target=calculate_speed)

        send_thread.start()
        receive_thread.start()
        speed_thread.start()

        send_thread.join()
        receive_thread.join()
        speed_thread.join()

    print("\nSerial communication ended")
